import picking.*;
import peasy.test.*;
import peasy.org.apache.commons.math.*;
import peasy.*;
import peasy.org.apache.commons.math.geometry.*;

PeasyCam cam;
Picker picker;
ArrayList spheres;
ArrayList suelo;
int id=-1;
int estado=40;
boolean debug = false;
Sphere seleccionada=null;
float mx,my,px,py,pz;
PFont font;
PVector gravedad=new PVector(0,0,0.0);
Spring s;
ArrayList restriccionTemporal;
long t1,t2;
ArrayList constraints;
ArrayList angles,hinges;

void setup() {
  size(800,600,P3D);
  lights();
  cam = new PeasyCam(this,width/2,height,height/2,100);
  cam.setMouseControlled(false);
  cam.setDistance(900);
  picker=new Picker(this);
  hinges=new ArrayList();
  spheres = new ArrayList();
  constraints = new ArrayList();
  suelo = new ArrayList();
  angles = new ArrayList();
 
  font = loadFont("PoorRichard-Regular-30.vlw");
  restriccionTemporal = new ArrayList();
  
  crearRag();
 
  //arriba
  //suelo.add(new Cube(new PVector(width,10,height),new PVector(width/2,0,0)));
  //abajo
  suelo.add(new Cube(new PVector(width,10,height),new PVector(width/2,height+170,0)));
  
  //izqueirda
  //suelo.add(new Cube(new PVector(10,height,height),new PVector(0,height/2,0)));
  //derecha
 // suelo.add(new Cube(new PVector(10,height,height),new PVector(width,height/2,0)));
  //fondo
//  suelo.add(new Cube(new PVector(width,height,10),new PVector(width/2,height/2,-height/2)));
  
  suelo.add(new Cube(new PVector(100,100,height),new PVector(width/2,height+100,0)));

  t1=System.currentTimeMillis()/1000;

  //frameRate(4);
  
  
   //cam.lookAt(width/2,height/2,0);
}


void crearRag(){
  float altura = height-100;
  
  //Torso
 Sphere hombroZ =  new Sphere(5,new PVector(width/2,altura*0.130,20), new PVector(0,0,0),true);
 Sphere hombroXneg =  new Sphere(5,new PVector(width/2+(altura*0.129),altura*0.130,0), new PVector(0,0,0),debug);
 Sphere hombroXpos =  new Sphere(5,new PVector(width/2-(altura*0.129),altura*0.130,0), new PVector(0,0,0),debug);
 Sphere hombroY =  new Sphere(5,new PVector(width/2,altura*0.260,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(hombroZ,hombroXneg,true));
 constraints.add(new Stick(hombroXneg,hombroXpos,true));
 constraints.add(new Stick(hombroZ,hombroXpos,true));
 
 constraints.add(new Stick(hombroZ,hombroY,true));
 constraints.add(new Stick(hombroXneg,hombroY,true));
 constraints.add(new Stick(hombroXpos,hombroY,true));
 
 spheres.add(hombroZ);
 spheres.add(hombroXneg);
 spheres.add(hombroXpos);
 spheres.add(hombroY);
 
 
 //Cintura
 
 Sphere cinturaZ=new Sphere(5,new PVector(width/2,altura*0.520,20), new PVector(0,0,0),debug);
 Sphere cinturaXneg=new Sphere(5,new PVector(width/2-(altura*0.191/2),altura*0.520,0), new PVector(0,0,0),debug);
 Sphere cinturaXpos=new Sphere(5,new PVector(width/2+(altura*0.191/2),altura*0.520,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(cinturaZ,hombroY,true));
 constraints.add(new Stick(cinturaXneg,hombroY,true));
 constraints.add(new Stick(cinturaXpos,hombroY,true));
 
 constraints.add(new Stick(cinturaZ,cinturaXneg,true));
 constraints.add(new Stick(cinturaXneg,cinturaXpos,true));
 constraints.add(new Stick(cinturaXpos,cinturaZ,true));
 
 spheres.add(cinturaZ);
 spheres.add(cinturaXneg);
 spheres.add(cinturaXpos);
 
// angles.add(new Angle(hombroXpos,hombroY,cinturaXneg,cinturaXpos,cinturaZ,45,new PVector(0,1,0),restriccionTemporal));
 //angles.add(new Angle(cinturaXpos,hombroY,hombroXneg,hombroXpos,hombroZ,45,new PVector(0,1,0),restriccionTemporal));
 
 //Brazo derecho
 
 Sphere brazoderZ=new Sphere(5,new PVector(width/2+altura*0.315 ,altura*0.130,20), new PVector(0,0,0),debug);
 Sphere brazoderXneg=new Sphere(5,new PVector(width/2+altura*0.315 ,altura*0.130-20,0), new PVector(0,0,0),debug);
 Sphere brazoderXpos=new Sphere(5,new PVector(width/2+altura*0.315 ,altura*0.130+20,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(brazoderZ,hombroXneg,true));
 constraints.add(new Stick(brazoderXneg,hombroXneg,true));
 constraints.add(new Stick(brazoderXpos,hombroXneg,true));
 
 constraints.add(new Stick(brazoderZ,brazoderXneg,true));
 constraints.add(new Stick(brazoderXneg,brazoderXpos,true));
 constraints.add(new Stick(brazoderXpos,brazoderZ,true));
 
 
 angles.add(new Angle(hombroXpos,hombroXneg,brazoderXpos,brazoderXneg,brazoderZ,89,new PVector(1,0,0),restriccionTemporal));
 
 
 
 spheres.add(brazoderZ);
 spheres.add(brazoderXneg);
 spheres.add(brazoderXpos);
 
 //Brazoizq
 
 Sphere brazoizqZ=new Sphere(5,new PVector(width/2-altura*0.315 ,altura*0.130,20), new PVector(0,0,0),debug);
 Sphere brazoizqXneg=new Sphere(5,new PVector(width/2-altura*0.315 ,altura*0.130-20,0), new PVector(0,0,0),debug);
 Sphere brazoizqXpos=new Sphere(5,new PVector(width/2-altura*0.315 ,altura*0.130+20,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(brazoizqZ,hombroXpos,true));
 constraints.add(new Stick(brazoizqXneg,hombroXpos,true));
 constraints.add(new Stick(brazoizqXpos,hombroXpos,true));
 
 constraints.add(new Stick(brazoizqZ,brazoizqXneg,true));
 constraints.add(new Stick(brazoizqXneg,brazoizqXpos,true));
 constraints.add(new Stick(brazoizqXpos,brazoizqZ,true));
 
 angles.add(new Angle(hombroXneg,hombroXpos,brazoizqXpos,brazoizqXneg,brazoizqZ,89,new PVector(-1,0,0),restriccionTemporal));
 
 spheres.add(brazoizqZ);
 spheres.add(brazoizqXneg);
 spheres.add(brazoizqXpos);
 

 
 //AntebrazoIzquierdo
 
 Sphere antebrazoizqZ=new Sphere(5,new PVector(width/2-altura*0.315-20 ,altura*0.130,20), new PVector(0,0,0),debug);
 Sphere antebrazoizqY=new Sphere(5,new PVector(width/2 -altura*0.461,altura*0.130,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(antebrazoizqZ,brazoizqXneg,true));
 constraints.add(new Stick(antebrazoizqZ,brazoizqXpos,true));
 
 
 constraints.add(new Stick(antebrazoizqZ,antebrazoizqY,true));
 constraints.add(new Stick(brazoizqXneg,antebrazoizqY,true));
 constraints.add(new Stick(brazoizqXpos,antebrazoizqY,true));
 
 hinges.add(new Hinge(hombroXpos,brazoizqXpos,antebrazoizqY,135,antebrazoizqZ,brazoizqXneg,brazoizqZ));

 spheres.add(antebrazoizqZ);
 spheres.add(antebrazoizqY);
 
 //AntebrazoDerecho
 
 Sphere antebrazoderZ=new Sphere(5,new PVector(width/2+altura*0.315+20 ,altura*0.130,20), new PVector(0,0,0),debug);
 Sphere antebrazoderY=new Sphere(5,new PVector(width/2 +altura*0.461,altura*0.130,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(antebrazoderZ,brazoderXneg,true));
 constraints.add(new Stick(antebrazoderZ,brazoderXpos,true));
 
 
 constraints.add(new Stick(antebrazoderZ,antebrazoderY,true));
 constraints.add(new Stick(brazoderXneg,antebrazoderY,true));
 constraints.add(new Stick(brazoderXpos,antebrazoderY,true));
 
hinges.add(new Hinge(hombroXneg,brazoderXpos,antebrazoderY,135,antebrazoderZ,brazoderXneg,brazoderZ));
 
 spheres.add(antebrazoderZ);
 spheres.add(antebrazoderY);
 
 //pantorrilla izquierda
 
 Sphere pantorrillaizqZ=new Sphere(5,new PVector(width/2 -(altura*0.191/2),altura*0.715,20), new PVector(0,0,0),debug);
 Sphere pantorrillaizqXneg=new Sphere(5,new PVector(width/2 -(altura*0.191/2)+20,altura*0.715,0), new PVector(0,0,0),debug);
 Sphere pantorrillaizqXpos=new Sphere(5,new PVector(width/2 -(altura*0.191/2)-20,altura*0.715,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(pantorrillaizqZ,cinturaXneg,true));
 constraints.add(new Stick(pantorrillaizqXneg,cinturaXneg,true));
 constraints.add(new Stick(pantorrillaizqXpos,cinturaXneg,true));
 
 constraints.add(new Stick(pantorrillaizqZ,pantorrillaizqXneg,true));
 constraints.add(new Stick(pantorrillaizqXneg,pantorrillaizqXpos,true));
 constraints.add(new Stick(pantorrillaizqXpos,pantorrillaizqZ,true));
 
 angles.add(new Angle(hombroY,cinturaXneg,pantorrillaizqXpos,pantorrillaizqXneg,pantorrillaizqZ,89,new PVector(0,1,0),restriccionTemporal));
 
 spheres.add(pantorrillaizqZ);
 spheres.add(pantorrillaizqXneg);
 spheres.add(pantorrillaizqXpos);
 
 //pantorrilla derecha
 
 Sphere pantorrilladerZ=new Sphere(5,new PVector(width/2 +(altura*0.191/2),altura*0.715,20), new PVector(0,0,0),debug);
 Sphere pantorrilladerXneg=new Sphere(5,new PVector(width/2 +(altura*0.191/2)+20,altura*0.715,0), new PVector(0,0,0),debug);
 Sphere pantorrilladerXpos=new Sphere(5,new PVector(width/2 +(altura*0.191/2)-20,altura*0.715,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(pantorrilladerZ,cinturaXpos,true));
 constraints.add(new Stick(pantorrilladerXneg,cinturaXpos,true));
 constraints.add(new Stick(pantorrilladerXpos,cinturaXpos,true));
 
 constraints.add(new Stick(pantorrilladerZ,pantorrilladerXneg,true));
 constraints.add(new Stick(pantorrilladerXneg,pantorrilladerXpos,true));
 constraints.add(new Stick(pantorrilladerXpos,pantorrilladerZ,true));
 
 spheres.add(pantorrilladerZ);
 spheres.add(pantorrilladerXneg);
 spheres.add(pantorrilladerXpos);
 
 
  angles.add(new Angle(hombroY,cinturaXpos,pantorrilladerXpos,pantorrilladerXneg,pantorrilladerZ,89,new PVector(0,1,0),restriccionTemporal));
 
 //gemelo izquierdo
 
 Sphere gemeloizqZ=new Sphere(5,new PVector(width/2 -(altura*0.191/2),altura*0.715+20,20), new PVector(0,0,0),debug);
 Sphere gemeloizqY=new Sphere(5,new PVector(width/2 -(altura*0.191/2),altura*0.961,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(gemeloizqZ,pantorrillaizqXneg,true));
 constraints.add(new Stick(gemeloizqZ,pantorrillaizqXpos,true));
 
 
 constraints.add(new Stick(gemeloizqZ,gemeloizqY,true));
 constraints.add(new Stick(pantorrillaizqXneg,gemeloizqY,true));
 constraints.add(new Stick(pantorrillaizqXpos,gemeloizqY,true));
 
 hinges.add(new Hinge(cinturaXpos,pantorrillaizqXpos,gemeloizqY,135,pantorrillaizqZ,pantorrillaizqXneg,gemeloizqZ));
 
 spheres.add(gemeloizqZ);
 spheres.add(gemeloizqY);
 
 
 //gemelo derecho
 
 Sphere gemeloderZ=new Sphere(5,new PVector(width/2 +(altura*0.191/2),altura*0.715+20,20), new PVector(0,0,0),debug);
 Sphere gemeloderY=new Sphere(5,new PVector(width/2 +(altura*0.191/2),altura*0.961,0), new PVector(0,0,0),debug);
 
 constraints.add(new Stick(gemeloderZ,pantorrilladerXneg,true));
 constraints.add(new Stick(gemeloderZ,pantorrilladerXpos,true));
 
 
 constraints.add(new Stick(gemeloderZ,gemeloderY,true));
 constraints.add(new Stick(pantorrilladerXneg,gemeloderY,true));
 constraints.add(new Stick(pantorrilladerXpos,gemeloderY,true));
 
 hinges.add(new Hinge(cinturaXneg,pantorrilladerXpos,gemeloderY,135,pantorrilladerZ,pantorrilladerXneg,gemeloderZ));
 
 spheres.add(gemeloderZ);
 spheres.add(gemeloderY);
 
}


void hud(){
  fill(255);
  cam.beginHUD();
 textFont(font);
 text("Controls:",100,100);
 text("W : Top view",100,150);
 text("S : Normal view",100,200);
 text("A : Right view", 100,250); 
 text("D : Left view",100,300);
 cam.endHUD();
}

void mousePressed(){
  crearRagdoll();
 
  float altura = height;
  if(estado==0){
 Sphere hombroZ =  new Sphere(5,new PVector(mouseX,mouseY+altura*0.130,20), new PVector(0,0,0),true);
 Sphere hombroXneg =  new Sphere(5,new PVector(mouseX+(altura*0.129),mouseY+altura*0.130,0), new PVector(0,0,0),true);
 hombroXneg.setDistancia(altura*0.129);
 Sphere hombroXpos =  new Sphere(5,new PVector(mouseX-(altura*0.129),mouseY+altura*0.130,0), new PVector(0,0,0),true);
 hombroXpos.setDistancia(altura*0.129);
 Sphere hombroY =  new Sphere(5,new PVector(mouseX,mouseY+altura*0.260,0), new PVector(0,0,0),true);
 hombroY.setDistancia(altura*(0.260-0.130));
 
 /*
 constraints.add(new Stick(hombroZ,hombroXneg,true));
 constraints.add(new Stick(hombroXneg,hombroXpos,true));
 constraints.add(new Stick(hombroZ,hombroXpos,true));
 
 constraints.add(new Stick(hombroZ,hombroY,true));
 constraints.add(new Stick(hombroXneg,hombroY,true));
 constraints.add(new Stick(hombroXpos,hombroY,true));*/
 
 spheres.add(hombroZ); //0
 spheres.add(hombroXneg); //1
 spheres.add(hombroXpos); //2
 spheres.add(hombroY);  //3
  }
  
  if(estado==1){
 constraints.add(new Stick((Sphere)spheres.get(0),(Sphere)spheres.get(1),true));
 constraints.add(new Stick((Sphere)spheres.get(1),(Sphere)spheres.get(2),true));
 constraints.add(new Stick((Sphere)spheres.get(2),(Sphere)spheres.get(0),true));
 constraints.add(new Stick((Sphere)spheres.get(0),(Sphere)spheres.get(3),true));
 constraints.add(new Stick((Sphere)spheres.get(1),(Sphere)spheres.get(3),true));
 constraints.add(new Stick((Sphere)spheres.get(2),(Sphere)spheres.get(3),true));
 
 
 
 
 Sphere cinturaZ=new Sphere(5,new PVector(mouseX,mouseY+altura*0.520,20), new PVector(0,0,0),true);
 cinturaZ.setDistancia(altura*(0.520-0.260));
 Sphere cinturaXneg=new Sphere(5,new PVector(mouseX-(altura*0.191/2),mouseY+altura*0.520,0), new PVector(0,0,0),true);
 cinturaXneg.setDistancia(altura*0.191/2);
 Sphere cinturaXpos=new Sphere(5,new PVector(mouseX+(altura*0.191/2),mouseY+altura*0.520,0), new PVector(0,0,0),true);
 cinturaXpos.setDistancia(altura*0.191/2);
 
 
 spheres.add(cinturaZ); //4
 spheres.add(cinturaXneg); //5
 spheres.add(cinturaXpos); //6
 
 
  }
  if(estado==2){
 constraints.add(new Stick((Sphere)spheres.get(4),(Sphere)spheres.get(5),true));
 constraints.add(new Stick((Sphere)spheres.get(5),(Sphere)spheres.get(6),true));
 constraints.add(new Stick((Sphere)spheres.get(6),(Sphere)spheres.get(4),true));
 
 constraints.add(new Stick((Sphere)spheres.get(4),(Sphere)spheres.get(3),true));
 constraints.add(new Stick((Sphere)spheres.get(5),(Sphere)spheres.get(3),true));
 constraints.add(new Stick((Sphere)spheres.get(6),(Sphere)spheres.get(3),true));
 
 
 Sphere brazoderZ=new Sphere(5,new PVector(mouseX+altura*0.315 ,mouseY+altura*0.130,20), new PVector(0,0,0),true);
 brazoderZ.setDistancia(altura*(0.315-0.129));
 Sphere brazoderXneg=new Sphere(5,new PVector(mouseX+altura*0.315 ,mouseY+altura*0.130-20,0), new PVector(0,0,0),true);
 brazoderXneg.setDistancia(20);
 Sphere brazoderXpos=new Sphere(5,new PVector(mouseX+altura*0.315 ,mouseY+altura*0.130+20,0), new PVector(0,0,0),true);
 brazoderXpos.setDistancia(20);
 
 Sphere hombroXpos = (Sphere)spheres.get(2);
 Sphere hombroXneg = (Sphere)spheres.get(1);
 angles.add(new Angle(hombroXpos,hombroXneg,brazoderXpos,brazoderXneg,brazoderZ,89,new PVector(1,0,0),restriccionTemporal));
 
 spheres.add(brazoderZ); //7
 spheres.add(brazoderXneg); //8
 spheres.add(brazoderXpos); //9
 
 constraints.add(new Stick((Sphere)spheres.get(7),(Sphere)spheres.get(8),true));
 constraints.add(new Stick((Sphere)spheres.get(8),(Sphere)spheres.get(9),true));
 constraints.add(new Stick((Sphere)spheres.get(7),(Sphere)spheres.get(9),true));
 
  }
  if(estado==3){
 constraints.add(new Stick((Sphere)spheres.get(7),(Sphere)spheres.get(1),true));
 constraints.add(new Stick((Sphere)spheres.get(8),(Sphere)spheres.get(1),true));
 constraints.add(new Stick((Sphere)spheres.get(9),(Sphere)spheres.get(1),true));
 
 
  
  //AntebrazoDerecho
 Sphere s = (Sphere) spheres.get(7);
 PVector v = s.getVector();
 
 Sphere antebrazoderZ=new Sphere(5,new PVector(v.x+20 ,v.y,20), new PVector(0,0,0),true);
 Sphere antebrazoderY=new Sphere(5,new PVector(mouseX +altura*0.461,mouseY+altura*0.130,0), new PVector(0,0,0),true);
 antebrazoderY.setDistancia(altura*(0.461-0.315));
 
 
 Sphere hombroXneg = (Sphere) spheres.get(1);
 Sphere brazoderXpos = (Sphere) spheres.get(9);
 Sphere brazoderXneg = (Sphere) spheres.get(8);
 Sphere brazoderZ = (Sphere) spheres.get(7);
 hinges.add(new Hinge(hombroXneg,brazoderXpos,antebrazoderY,135,antebrazoderZ,brazoderXneg,brazoderZ));
 
 spheres.add(antebrazoderZ); //10
 spheres.add(antebrazoderY); //11
 
 
 constraints.add(new Stick((Sphere)spheres.get(10),(Sphere)spheres.get(8),true));
 constraints.add(new Stick((Sphere)spheres.get(10),(Sphere)spheres.get(9),true));
 
 

  }
  if(estado==4){
  constraints.add(new Stick((Sphere)spheres.get(10),(Sphere)spheres.get(11),true));
 constraints.add(new Stick((Sphere)spheres.get(8),(Sphere)spheres.get(11),true));
 constraints.add(new Stick((Sphere)spheres.get(9),(Sphere)spheres.get(11),true));
 
 Sphere brazoizqZ=new Sphere(5,new PVector(mouseX-altura*0.315 ,mouseY+altura*0.130,20), new PVector(0,0,0),true);
 brazoizqZ.setDistancia(altura*(0.315-0.129));
 Sphere brazoizqXneg=new Sphere(5,new PVector(mouseX-altura*0.315 ,mouseY+altura*0.130-20,0), new PVector(0,0,0),true);
 brazoizqXneg.setDistancia(20);
 Sphere brazoizqXpos=new Sphere(5,new PVector(mouseX-altura*0.315 ,mouseY+altura*0.130+20,0), new PVector(0,0,0),true);
 brazoizqXpos.setDistancia(20);
 
 Sphere hombroXpos = (Sphere)spheres.get(2);
 Sphere hombroXneg = (Sphere)spheres.get(1);
 
 angles.add(new Angle(hombroXneg,hombroXpos,brazoizqXpos,brazoizqXneg,brazoizqZ,89,new PVector(-1,0,0),restriccionTemporal));
 
 spheres.add(brazoizqZ);//12
 spheres.add(brazoizqXneg);//13
 spheres.add(brazoizqXpos);//14
 
 
 
 constraints.add(new Stick((Sphere)spheres.get(12),(Sphere)spheres.get(13),true));
 constraints.add(new Stick((Sphere)spheres.get(13),(Sphere)spheres.get(14),true));
 constraints.add(new Stick((Sphere)spheres.get(14),(Sphere)spheres.get(12),true));
 
  }
  if(estado==5){


 
 
 
 constraints.add(new Stick((Sphere)spheres.get(12),(Sphere)spheres.get(2),true));
 constraints.add(new Stick((Sphere)spheres.get(13),(Sphere)spheres.get(2),true));
 constraints.add(new Stick((Sphere)spheres.get(14),(Sphere)spheres.get(2),true));
 
 Sphere s = (Sphere) spheres.get(12);
 PVector v = s.getVector();
 
 Sphere antebrazoizqZ=new Sphere(5,new PVector(v.x-20 ,v.y,20), new PVector(0,0,0),true);
 Sphere antebrazoizqY=new Sphere(5,new PVector(mouseX -altura*0.461,mouseY+altura*0.130,0), new PVector(0,0,0),true);
 antebrazoizqY.setDistancia(altura*(0.461-0.315));
 
 Sphere hombroXpos = (Sphere) spheres.get(2);
 Sphere brazoizqXpos = (Sphere) spheres.get(14);
 Sphere brazoizqXneg = (Sphere) spheres.get(13);
 Sphere brazoizqZ = (Sphere) spheres.get(12);
 hinges.add(new Hinge(hombroXpos,brazoizqXpos,antebrazoizqY,135,antebrazoizqZ,brazoizqXneg,brazoizqZ));
 
 spheres.add(antebrazoizqZ);//15
 spheres.add(antebrazoizqY);//16
    
  }
  if(estado==6){
 
    
  constraints.add(new Stick((Sphere)spheres.get(15),(Sphere)spheres.get(13),true));
 constraints.add(new Stick((Sphere)spheres.get(15),(Sphere)spheres.get(14),true));
 
 
 constraints.add(new Stick((Sphere)spheres.get(15),(Sphere)spheres.get(16),true));
 constraints.add(new Stick((Sphere)spheres.get(13),(Sphere)spheres.get(16),true));
 constraints.add(new Stick((Sphere)spheres.get(14),(Sphere)spheres.get(16),true));
 
 
 Sphere pantorrillaizqZ=new Sphere(5,new PVector(mouseX -(altura*0.191/2),mouseY+altura*0.715,20), new PVector(0,0,0),true);
 pantorrillaizqZ.setDistancia(altura*(0.715-0.520));
 Sphere pantorrillaizqXneg=new Sphere(5,new PVector(mouseX -(altura*0.191/2)+20,mouseY+altura*0.715,0), new PVector(0,0,0),true);
 pantorrillaizqXneg.setDistancia(20);
 Sphere pantorrillaizqXpos=new Sphere(5,new PVector(mouseX -(altura*0.191/2)-20,mouseY+altura*0.715,0), new PVector(0,0,0),true);
 pantorrillaizqXpos.setDistancia(20);


 Sphere hombroY =(Sphere) spheres.get(3);
 Sphere cinturaXneg = (Sphere) spheres.get(5);
 angles.add(new Angle(hombroY,cinturaXneg,pantorrillaizqXpos,pantorrillaizqXneg,pantorrillaizqZ,89,new PVector(0,1,0),restriccionTemporal));
 
 
 spheres.add(pantorrillaizqZ);//17
 spheres.add(pantorrillaizqXneg);//18
 spheres.add(pantorrillaizqXpos);//19

 constraints.add(new Stick((Sphere)spheres.get(17),(Sphere)spheres.get(18),true));
 constraints.add(new Stick((Sphere)spheres.get(18),(Sphere)spheres.get(19),true));
 constraints.add(new Stick((Sphere)spheres.get(19),(Sphere)spheres.get(17),true));

}
if(estado==7){
constraints.add(new Stick((Sphere)spheres.get(17),(Sphere)spheres.get(5),true));
 constraints.add(new Stick((Sphere)spheres.get(18),(Sphere)spheres.get(5),true));
 constraints.add(new Stick((Sphere)spheres.get(19),(Sphere)spheres.get(5),true));
 
 
 
 
 Sphere s = (Sphere) spheres.get(17);
 PVector v = s.getVector();
 Sphere gemeloizqZ=new Sphere(5,new PVector(v.x,v.y+20,20), new PVector(0,0,0),true);
 
 Sphere gemeloizqY=new Sphere(5,new PVector(mouseX -(altura*0.191/2),mouseY+altura*0.961,0), new PVector(0,0,0),true);
 gemeloizqY.setDistancia(altura*(0.961-0.715));

 
 
 spheres.add(gemeloizqZ);//20
 spheres.add(gemeloizqY);//21
}
if(estado==8){
constraints.add(new Stick((Sphere)spheres.get(20),(Sphere)spheres.get(18),true));
 constraints.add(new Stick((Sphere)spheres.get(20),(Sphere)spheres.get(19),true));
 
 
 constraints.add(new Stick((Sphere)spheres.get(20),(Sphere)spheres.get(21),true));
 constraints.add(new Stick((Sphere)spheres.get(18),(Sphere)spheres.get(21),true));
 constraints.add(new Stick((Sphere)spheres.get(19),(Sphere)spheres.get(21),true));
 
 
  Sphere pantorrilladerZ=new Sphere(5,new PVector(mouseX +(altura*0.191/2),mouseY+altura*0.715,20), new PVector(0,0,0),true);
  pantorrilladerZ.setDistancia(altura*(0.715-0.520));
 Sphere pantorrilladerXneg=new Sphere(5,new PVector(mouseX +(altura*0.191/2)+20,mouseY+altura*0.715,0), new PVector(0,0,0),true);
 pantorrilladerXneg.setDistancia(20);
 Sphere pantorrilladerXpos=new Sphere(5,new PVector(mouseX +(altura*0.191/2)-20,mouseY+altura*0.715,0), new PVector(0,0,0),true);
 pantorrilladerXpos.setDistancia(20);
 
 Sphere hombroY =(Sphere) spheres.get(3);
 Sphere cinturaXpos = (Sphere) spheres.get(6);
 angles.add(new Angle(hombroY,cinturaXpos,pantorrilladerXpos,pantorrilladerXneg,pantorrilladerZ,89,new PVector(0,1,0),restriccionTemporal));
 
 spheres.add(pantorrilladerZ);//22
 spheres.add(pantorrilladerXneg);//23
 spheres.add(pantorrilladerXpos);//24
 
 constraints.add(new Stick((Sphere)spheres.get(22),(Sphere)spheres.get(23),true));
 constraints.add(new Stick((Sphere)spheres.get(23),(Sphere)spheres.get(24),true));
 constraints.add(new Stick((Sphere)spheres.get(24),(Sphere)spheres.get(22),true));
 
}
if(estado==9){
 constraints.add(new Stick((Sphere)spheres.get(22),(Sphere)spheres.get(6),true));
 constraints.add(new Stick((Sphere)spheres.get(23),(Sphere)spheres.get(6),true));
 constraints.add(new Stick((Sphere)spheres.get(24),(Sphere)spheres.get(6),true));
 
 
 
 
 Sphere s = (Sphere) spheres.get(22);
 PVector v = s.getVector();
 Sphere gemeloderZ=new Sphere(5,new PVector(v.x,v.y+20,20), new PVector(0,0,0),true);
 Sphere gemeloderY=new Sphere(5,new PVector(mouseX +(altura*0.191/2),mouseY+altura*0.961,0), new PVector(0,0,0),true);
 gemeloderY.setDistancia(altura*(0.961-0.715));
 
 
 
 
 spheres.add(gemeloderZ);//25
 spheres.add(gemeloderY);//26
 
}
if(estado==10){
 constraints.add(new Stick((Sphere)spheres.get(25),(Sphere)spheres.get(23),true));
 constraints.add(new Stick((Sphere)spheres.get(25),(Sphere)spheres.get(24),true));
 
 
 constraints.add(new Stick((Sphere)spheres.get(25),(Sphere)spheres.get(26),true));
 constraints.add(new Stick((Sphere)spheres.get(23),(Sphere)spheres.get(26),true));
 constraints.add(new Stick((Sphere)spheres.get(24),(Sphere)spheres.get(26),true)); 
}
if(estado==11){
 Iterator i = spheres.iterator();
while(i.hasNext()){
 Sphere s = (Sphere) i.next();
 s.setPinned(false); 
}
}

  estado++;
 
  
}
void mouseMoved(){
 
 if( estado==1){
  Sphere s = (Sphere) spheres.get(estado*3);
  Sphere s2 = (Sphere) spheres.get(estado*3-3);
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=0;
  s.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  v.z=0;
  m.sub(v);
  m.normalize();
  float x = m.x;
  float y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  Sphere s3 = (Sphere) spheres.get(estado*3-2);
  Sphere s4 = (Sphere) spheres.get(estado*3-1);
  m.mult(s3.getDistancia());
  m.add(s2.getVector());
  m.z=0;
  s3.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  m.sub(v);
  m.normalize();
  x = m.x;
  y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  m.mult(s4.getDistancia());
  m.mult(-1);
  m.add(s2.getVector());
  m.z=0;
  s4.setPosicion(m);
  
  
 // s.setPosicion(new PVector(mouseX,mouseY));
  
 }
 if(estado==2){
  Sphere s = (Sphere) spheres.get(4);
  Sphere s2 = (Sphere) spheres.get(3);
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=20;
  s.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  v.z=0;
  m.sub(v);
  m.normalize();
  float x = m.x;
  float y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  Sphere s3 = (Sphere) spheres.get(5);
  Sphere s4 = (Sphere) spheres.get(6);
  m.mult(s3.getDistancia());
  m.add(s.getVector());
  m.z=0;
  s3.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  m.sub(v);
  m.normalize();
  x = m.x;
  y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  m.mult(s4.getDistancia());
  m.mult(-1);
  m.add(s.getVector());
  m.z=0;
  s4.setPosicion(m);
   
 }
if(estado==3){
  
  
  Angle a = (Angle) angles.get(0);
  
  Sphere s = (Sphere) spheres.get(7);
  Sphere s2 = (Sphere) spheres.get(1);
  Sphere s3 = (Sphere) spheres.get(8);
  Sphere s4 = (Sphere) spheres.get(9);
  if(!a.apply()){
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=20;
  s.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  v.z=0;
  m.sub(v);
  m.normalize();
  float x = m.x;
  float y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  
  m.mult(s3.getDistancia());
  m.add(s.getVector());
  m.z=0;
  s3.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  m.sub(v);
  m.normalize();
  x = m.x;
  y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  m.mult(s4.getDistancia());
  m.mult(-1);
  m.add(s.getVector());
  m.z=0;
  s4.setPosicion(m);}
  
  
  
  
}
if(estado==4){
  Hinge h = (Hinge) hinges.get(0);
  Sphere s = (Sphere) spheres.get(11);
  Sphere s2 = (Sphere) spheres.get(10);
  
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=20;
  s.setPosicion(m);
  
  
}
if(estado==5){
  Angle a = (Angle) angles.get(1);
  Sphere s = (Sphere) spheres.get(12);
  Sphere s2 = (Sphere) spheres.get(2);
  //println(a.apply());
  if(!a.apply()){
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=20;
  s.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  v.z=0;
  m.sub(v);
  m.normalize();
  float x = m.x;
  float y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  Sphere s3 = (Sphere) spheres.get(13);
  Sphere s4 = (Sphere) spheres.get(14);
  m.mult(s3.getDistancia());
  m.add(s.getVector());
  m.z=0;
  s3.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  m.sub(v);
  m.normalize();
  x = m.x;
  y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  m.mult(s4.getDistancia());
  m.mult(-1);
  m.add(s.getVector());
  m.z=0;
  s4.setPosicion(m);}
  
}
if(estado==6){
  Hinge h = (Hinge) hinges.get(1);
  Sphere s = (Sphere) spheres.get(16);
  Sphere s2 = (Sphere) spheres.get(15);
  
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=20;
  s.setPosicion(m);
  
  
}

if(estado==7){
   Angle a = (Angle) angles.get(2);
  Sphere s = (Sphere) spheres.get(17);
  Sphere s2 = (Sphere) spheres.get(5);
  if(!a.apply()){
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=20;
  s.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  v.z=0;
  m.sub(v);
  m.normalize();
  float x = m.x;
  float y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  Sphere s3 = (Sphere) spheres.get(18);
  Sphere s4 = (Sphere) spheres.get(19);
  m.mult(s3.getDistancia());
  m.add(s.getVector());
  m.z=0;
  s3.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  m.sub(v);
  m.normalize();
  x = m.x;
  y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  m.mult(s4.getDistancia());
  m.mult(-1);
  m.add(s.getVector());
  m.z=0;
  s4.setPosicion(m);}
  
}
if(estado==8){
  Sphere s = (Sphere) spheres.get(21);
  Sphere s2 = (Sphere) spheres.get(20);
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=20;
  s.setPosicion(m);
  
  
}
if(estado==9){
  Angle a = (Angle) angles.get(3);
  Sphere s = (Sphere) spheres.get(22);
  Sphere s2 = (Sphere) spheres.get(6);
  if(!a.apply()){
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=20;
  s.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  v.z=0;
  m.sub(v);
  m.normalize();
  float x = m.x;
  float y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  Sphere s3 = (Sphere) spheres.get(23);
  Sphere s4 = (Sphere) spheres.get(24);
  m.mult(s3.getDistancia());
  m.add(s.getVector());
  m.z=0;
  s3.setPosicion(m);
  
  m = new PVector(mouseX,mouseY,0);
  
  m.sub(v);
  m.normalize();
  x = m.x;
  y = m.y;
  m.x=y;
  m.y=-x;
  m.z=0;
  
  m.mult(s4.getDistancia());
  m.mult(-1);
  m.add(s.getVector());
  m.z=0;
  s4.setPosicion(m);}
  
}
if(estado==10){
  Sphere s = (Sphere) spheres.get(26);
  Sphere s2 = (Sphere) spheres.get(25);
  PVector v2 = s2.getVector();
  PVector v = v2.get();
  v.z=0;
  PVector m = new PVector(mouseX,mouseY,0);
  m.sub(v);
  m.normalize();
  m.mult(s.getDistancia());
  m.add(s2.getVector());
  m.z=20;
  s.setPosicion(m);
  
  
}
 
}
void keyPressed(){
 if(key=='w'){
  cam.setRotations(0,0,0);
   float[] rot =  cam.getRotations();
  
  cam.rotateX(abs(rot[1]-radians(90)));
 }
 if(key=='s'){
  cam.setRotations(0,0,0);
 }
 if(key=='a'){
   cam.setRotations(0,0,0);
  float[] rot =  cam.getRotations();
  
 cam.rotateY(abs(rot[1]-radians(90)));
  
 }
 if(key=='d'){
   cam.setRotations(0,0,0);
   float[] rot =  cam.getRotations();
  
  cam.rotateY(-abs(rot[1]-radians(-90)));
  
 }
 if(key=='q'){
   
  cam.setDistance(cam.getDistance()+100); 
  println(cam.getDistance());
 }
 if(key=='e'){
  cam.setDistance(cam.getDistance()-100); 
 }
 
 if(key=='o'){
   if(gravedad.y!=0.98)
  gravedad.add(new PVector(0,0.98,0));
 }
 if(key=='l'){
   if(gravedad.y!=-0.98)
  gravedad.add(new PVector(0,-0.98,0));
 }
 if(key=='k'){
   if(gravedad.x!=0.98)
  gravedad.add(new PVector(0.98,0,0));
 }
 if(key=='ñ'){
   if(gravedad.x!=-0.98)
  gravedad.add(new PVector(-0.98,0,0));
 }
}

void crearRagdoll() {
  
  
    mx=mouseX;
    my=mouseY;
    
    id = picker.get(mouseX,mouseY);
    println("entro");
  if(id>-1) {
    println(id);
    seleccionada = (Sphere)spheres.get(id);
   //seleccionada.setPinned(true);
    
    s = new Spring(seleccionada);
    seleccionada.setPinned(false);
  }
  
}

void draw() {
   
  background(0);
  t2 =  System.currentTimeMillis()/1000;
  gravedad();

  
  t1=t2;
  //colision_sin_damping();
  accelerar(1); 
  for(int i = 0; i< 20 ;i++) {
   
  
   
   bordeColision();
   restriccion();
   
   
   colisionStickCamilla();
   
   
   colision();
   colisionStick();
   
  }

  display();
  hud();
}

public void bordeColision() {
  Sphere s1;
  for(int i=0;i<spheres.size();i++) {
    s1 = (Sphere)spheres.get(i);

    
    float x = s1.getX();
    float y = s1.getY();
    float z = s1.getZ();

    float xtmp=x;
    float ytmp=y;
    float ztmp=z;

    x = min(max(x,0),width);
    y = min(max(y,0),height+150);
    z = min(max(z,-height/2 +10),height/2);

    s1.setPosicion(new PVector(x,y,z));
    
    
}
}

public void restriccion() {

  
  Iterator i = constraints.iterator();
  while(i.hasNext()) {
    Constraint c = (Constraint) i.next();
    c.apply();
  }
  
  
  
  i = angles.iterator();
  while(i.hasNext()) {
    Constraint c = (Constraint) i.next();
    c.apply();
  }
  
  i = hinges.iterator();
  while(i.hasNext()) {
    Constraint c = (Constraint) i.next();
    //c.apply();
  }
  
  i = restriccionTemporal.iterator();
  while(i.hasNext()) {
    Constraint c = (Constraint) i.next();
    c.apply();
  }

}

public void gravedad() {
  Iterator i = spheres.iterator();
  while(i.hasNext()) {
    Sphere s = (Sphere) i.next();
    if(!s.isPinned()) {
      s.setAcc(gravedad);
    }
  }
}
public void accelerar(float dt) {
  Iterator i = spheres.iterator();
  Sphere s1;
  while(i.hasNext()) {
    s1 = (Sphere) i.next();
    s1.actualizar(dt);
  }
}

public void colisionStickCamilla(){
  PVector vo = new PVector(width/2,height,0);
  
  for(int i = 0;i<constraints.size();i++){
   Stick st = (Stick) constraints.get(i);
  
   Sphere s1 = st.gets1();
   Sphere s2 = st.gets2();
   //Particle's psoitions
   PVector v1 = s1.getVector();
   PVector v2 = s2.getVector();
   PVector vold1 = s1.getPosicionAnterior();
   PVector vold2 = s2.getPosicionAnterior();
   
   if(inBox(v1)){
     println("V1 dentro");
     PVector proj = v1.get();
     proj.y = height+50;
     float d = proj.dist(v1);
     PVector mov = vold1.get();
     mov.sub(v1);
     mov.normalize();
     mov.mult(d);
     v1.add(mov);
     
   }
   else if(inBox(v2)){
     println("v2 dentro");
     PVector proj = v2.get();
     proj.y = height+50;
     //proj.x = abs(width/2-100 - proj.x) > abs(width/2+100 - proj.x) ? width/2+100:width/2-100; 
     float d = proj.dist(v2);
     PVector mov = vold2.get();
     mov.sub(v2);
     mov.normalize();
     mov.mult(d);
     v2.add(mov);
     
   }
   
   else{
    //Palo diagonal 
     PVector interX = interseccionPlano(new PVector(width/2,height+50,0),new PVector(0,-1,0),v1,v2);
     if(interX.x > width/2-50 && interX.x < width/2+50){
      
       PVector interYizq = interseccionPlano(new PVector(width/2-50,height+150,0),new PVector(-1,0,0),v1,v2);
       PVector interYder = interseccionPlano(new PVector(width/2+50,height+150,0),new PVector(1,0,0),v1,v2);
       
       if(interYizq.y > height+50 && interYizq.y < height+150){
         println("diagonal izquierda");
        PVector medio = interX.get();
        medio.add(interYizq);
        medio.div(2);
        PVector proj = medio.get();
        proj.x=width/2-50;
        proj.y=height+50;
        float distancia = medio.dist(proj);
        PVector mov = proj.get();
        mov.sub(medio);
        mov.normalize();
        
        float dist1 = v1.dist(medio);
        float dist2 = v2.dist(medio);
        float total = v1.dist(v2);
        dist1=dist1/total;
        dist2=dist2/total;
        PVector mov1 = mov.get();
        mov1.mult(distancia);
        mov1.mult(dist1);
        PVector mov2 = mov.get();
        mov2.mult(distancia);
        mov2.mult(dist2);
        v1.add(mov1);
        v2.add(mov2);
       }
       else if(interYder.y > height+50 && interYder.y < height+150){
         println("diagonal derecha");
        PVector medio = interX.get();
        medio.add(interYder);
        medio.div(2);
        PVector proj = medio.get();
        proj.x=width/2+50;
        proj.y=height+50;
        float distancia = medio.dist(proj);
        PVector mov = proj.get();
        mov.sub(medio);
        mov.normalize();
        
        float dist1 = v1.dist(medio);
        float dist2 = v2.dist(medio);
        float total = v1.dist(v2);
        dist1=dist1/total;
        dist2=dist2/total;
        PVector mov1 = mov.get();
        mov1.mult(distancia);
        mov1.mult(dist1);
        PVector mov2 = mov.get();
        mov2.mult(distancia);
        mov2.mult(dist2);
        v1.add(mov1);
        v2.add(mov2); 
       }
     }
     
     
   }
   
  }
  
  
}

public boolean inBox(PVector p){
 if(testFace(p,new PVector(width/2,height+50,0),new PVector(0,-1,0))){
   if(testFace(p,new PVector(width/2+50,height+100,0),new PVector(1,0,0))){
     if(testFace(p,new PVector(width/2-50,height+100,0),new PVector(-1,0,0))){
       if(testFace(p,new PVector(width/2,height+150,0),new PVector(0,1,0))){
         return true;
       }
     }
   }
 }
 return false;
  
}

public boolean testFace(PVector v1,PVector vo,PVector n){
  PVector startSide = v1.get();
  startSide.sub(vo);
  float side=startSide.dot(n);
  if(side<=0){
    return true;}
    else{
      return false;
    }
}

public boolean testPoint(PVector v1){
 PVector vo = new PVector(width/2,height+50,0);
  PVector startSide = v1.get();
 startSide.sub(vo);
 float side=startSide.dot(new PVector(0,-1,0));
 if(side <=0){
   vo = new PVector(width/2,height+150,0);
  startSide = v1.get();
 startSide.sub(vo);
   side = startSide.dot(new PVector(0,1,0));
   if(side<=0){
      vo = new PVector(width/2+50,height+100,0);
  startSide = v1.get();
 startSide.sub(vo);
     side = startSide.dot(new PVector(1,0,0));
   if(side<=0){
      vo = new PVector(width/2-50,height+100,0);
  startSide = v1.get();
 startSide.sub(vo);
     side = startSide.dot(new PVector(-1,0,0));
   if(side<=0){
      return true;
   }
   }
   }
   
 }
 return false;
}

public PVector interseccionPlano(PVector vo,PVector n,PVector v1,PVector v2){

   PVector u = v2.get();
   u.sub(v1);
   PVector w = v1.get();
   w.sub(vo);

   float D = n.dot(u);
   float N = -1* n.dot(w);

   if(abs(D)<0.001){
    if(N<=0){
      
     return new PVector(-1,-1,-1);
     
    }
     
   }
  float sI = N/D;
  if(sI<0||sI>1){
     return new PVector(0,0,0);}
  else{
  

 PVector i = v1.get();
 u.mult(sI);
 i.add(u);
 return i;
   
 }

  }

public void colisionStick(){
  
 for(int i=0;i<constraints.size();i++){
   Stick st= (Stick)constraints.get(i);
  for(int j=i+1;j<constraints.size();j++){
    
    
    Stick st2 = (Stick)constraints.get(j);
    
    Sphere s1 = st.gets1();
    Sphere s2 = st.gets2();
    Sphere s21 = st2.gets1();
    Sphere s22 = st2.gets2();
    
    
    
    PVector v1 = s1.getVector();
    PVector v2 = s2.getVector();
    PVector v21 = s21.getVector();
    PVector v22 = s22.getVector();
    
    if(!v1.equals(v2) && !v1.equals(v21) && !v1.equals(v22) && !v2.equals(v21) && !v2.equals(v22)){
    
    PVector u = v2.get();
    u.sub(v1);
    PVector v = v22.get();
    v.sub(v21);
    PVector w = v1.get();
    w.sub(v21);
    
    float a = u.dot(u);
    float b = u.dot(v);
    float c = v.dot(v);
    float d = u.dot(w);
    float e = v.dot(w);
    float D = a*c-b*b;
    float sc,sN,sD;
    sD=D;
    float tc,tN,tD;
    tD=D;
    
    if(D< 0.001){
     sN=0.0;
     sD=1.0;
    tN=e;
   tD=c;
    
    }
    else{
      sN=(b*e-c*d);
      tN=(a*e-b*d);
      if(sN<0.0){
        sN=0.0;
        tN=e;
        tD=c;
      }
      else if(sN>sD){
        sN=sD;
        tN=e+b;
        tD=c;
      }
    }
    if(tN<0.0){
      tN=0.0;
      if(-d<0.0){
        sN=0.0;}
          else if(-d > a){
            sN=sD;
          }
          else{
            sN=-d;
            sD=a;
          }
        }
        else if(tN>tD){
          tN=tD;
          if((-d+b)<0.0){
            sN=0;
          }
          else if((-d+b)>a){
            sN=sD;
          }else{
            sN=(-d+b);
            sD=a;
          }
        }
        sc=(abs(sN)<0.001?0.0:sN/sD);
        tc=(abs(tN)<0.001?0.0:tN/tD);
        
        u.mult(sc);
        v.mult(tc);
        PVector dp = w.get();
        dp.add(u);
        dp.sub(v);
        
          if(dp.mag() < 1){
            
            
            
            PVector pos1 = new PVector(0,0,0);
            pos1.add(s1.getPosicionAnterior());
            pos1.sub(v1);
            pos1.normalize();
            pos1.mult(2);
            pos1.add(v1);
            
            PVector pos2 = new PVector(0,0,0);
            pos2.add(s2.getPosicionAnterior());
            pos2.sub(v2);
            pos2.normalize();
            pos2.mult(2);
            pos2.add(v2);
            
            PVector pos21 = new PVector(0,0,0);
            pos21.add(s21.getPosicionAnterior());
            pos21.sub(v21);
            pos21.normalize();
            pos21.mult(2);
            pos21.add(v21);
            
            PVector pos22 = new PVector(0,0,0);
            pos22.add(s22.getPosicionAnterior());
            pos22.sub(v22);
            pos22.normalize();
            pos22.mult(2);
            pos22.add(v22);
            
            s1.setPosicion(pos1);
            s1.setPint(true);
            s2.setPosicion(pos2);
            s2.setPint(true);
            s21.setPosicion(pos21);
            s21.setPint(true);
            s22.setPosicion(pos22);
            s22.setPint(true);
        
          }
          else{
            s1.setPint(false);
            s2.setPint(false);
            s21.setPint(false);
            s22.setPint(false);
          }
      
    
     
        
          
        

    }
      
    
    
    
    
  }
  }
 
}

public void colision_sin_damping() {
  Sphere s1,s2;
  for(int i=0;i<spheres.size();i++) {
    s1 = (Sphere)spheres.get(i);
    for(int j=i+1;j<spheres.size();j++) {
      s2 = (Sphere)spheres.get(j);

      float x = s1.getX() -s2.getX();
      float y = s1.getY() - s2.getY();
      float z = s1.getZ() - s2.getZ();

      float slenght = x*x+y*y+z*z;
      float lenght = sqrt(slenght);
      float target = s1.getRadio()+s2.getRadio();

      if(lenght < target) {
        float factor = (lenght-target)/lenght;
        

        float x1 = s1.getX()-(x*factor*0.5);
        float y1 = s1.getY()-(y*factor*0.5);
        float z1 = s1.getZ()-(z*factor*0.5);
        float x2 = s2.getX()+(x*factor*0.5);
        float y2 = s2.getY()+(y*factor*0.5);
        float z2 = s2.getZ()+(z*factor*0.5);




        s1.setPosicion(new PVector(x1,y1,z1));
        s2.setPosicion(new PVector(x2,y2,z2));
      }
    }
  }
}

public void colision() {
  Sphere s1,s2;
  for(int i=0;i<spheres.size();i++) {
    s1 = (Sphere)spheres.get(i);
    for(int j=i+1;j<spheres.size();j++) {
      s2 = (Sphere)spheres.get(j);

      float x = s1.getX() -s2.getX();
      float y = s1.getY() - s2.getY();
      float z = s1.getZ() - s2.getZ();

      float slenght = x*x+y*y+z*z;
      float lenght = sqrt(slenght);
      float target = s1.getRadio()+s2.getRadio();

      if(lenght < target) {
       
        float factor = (lenght-target)/lenght;

        float x1,y1,z1,x2,y2,z2;
        
          x1 = s1.getX()-(x*factor*0.5);
          y1 = s1.getY()-(y*factor*0.5);
          z1 = s1.getZ()-(z*factor*0.5);
          x2 = s2.getX()+(x*factor*0.5);
          y2 = s2.getY()+(y*factor*0.5);
          z2 = s2.getZ()+(z*factor*0.5);
        
        

        float vx1 = s1.getX()-s1.getOX();
        float vy1 = s1.getY()-s1.getOY();
        float vz1 = s1.getZ()-s1.getOZ();
        float vx2 = s2.getX()-s2.getOX();
        float vy2 = s2.getY()-s2.getOY();
        float vz2 = s2.getZ()-s2.getOZ();

        float f1 = (0.99*(x*vx1+y*vy1+z*vz1))/slenght;
        float f2 = (0.99*(x*vx2+y*vy2+z*vz2))/slenght;

        vx1+=f2*x-f1*x;
        vy1+=f2*y-f1*y;
        vz1+=f2*z-f1*z;

        vx2+=f1*x-f2*x;
        vy2+=f1*y-f2*y;
        vz2+=f1*z-f2*z;

        
          s1.setPosicion(new PVector(x1,y1,z1));
        
        
          s2.setPosicion(new PVector(x2,y2,z2));
        


        float ox1 = s1.getX()-vx1;
        float oy1 = s1.getY()-vy1;
        float oz1 = s1.getZ()-vz1;
        float ox2 = s2.getX()-vx2;
        float oy2 = s2.getY()-vy2;
        float oz2 = s2.getZ()-vz2;

        
          s1.setPosicionAnterior(new PVector(x1,y1,z1));
        
      
        s2.setPosicionAnterior(new PVector(x2,y2,z2));
        
      }
    }
  }
}
public void inercia(long dt) {
}
public void display() {
  lights();

  Sphere s1;
  int count=1;
  for(int i=0;i<spheres.size();i++) {

    picker.start(i);
    s1 = (Sphere) spheres.get(i);
    s1.draw();
    count++;
  }

  Iterator i = constraints.iterator();
  while(i.hasNext()) {
    Constraint c = (Constraint) i.next();
    c.draw();
  }




  i = suelo.iterator();
  fill(200,200,0);
  while(i.hasNext()) {
    Cube s = (Cube) i.next();

    s.draw();
  }

  
}


void mouseDragged() {
  
  if(id>-1) {
   float[] rot = cam.getRotations();
   for(int i=0;i<rot.length;i++){
    println(i+" "+degrees(rot[i])); 
   }
   if(rot[1] > 0  && rot[0]==0 && rot[2]==0){
     seleccionada.setZ(pz - (mouseX-mx));
    seleccionada.setY(py + (mouseY-my));
   }
   else if(rot[1] < 0 && rot[0]==0 && rot[2]==0){
     seleccionada.setZ(pz + (mouseX-mx));
    seleccionada.setY(py + (mouseY-my));
   }
   else if(rot[0] < 0 && rot[2] < 0){
     seleccionada.setX(px - (mouseX-mx));
    seleccionada.setY(py + (mouseY-my));
    
   }
   else if(rot[0] >0 && rot[1]==0 && rot[2]==0){
    
     seleccionada.setX(px + (mouseX-mx));
     seleccionada.setZ(pz + (mouseY-my));
   }
   else if(rot[0] <0 && rot[1]==0 && rot[2]==0){
     seleccionada.setX(px + (mouseX-mx));
    seleccionada.setY(py - (mouseY-my));
   }
   else{
     if(s!=null){
     s.apply(); }
    //seleccionada.setX(px + (mouseX-mx));
   // seleccionada.setY(py + (mouseY-my));}
   // seleccionada.setOX(seleccionada.getX() + (mx-mouseX));
   // seleccionada.setOY(seleccionada.getY() + (my-mouseY));
  }
}
}


void mouseReleased() {
  if(id > -1){
      s=null;
     
   /* seleccionada.setOX(seleccionada.getX() + (mouseX-mx));
    seleccionada.setOY(seleccionada.getY() + (mouseY-my));
    seleccionada.setPinned(false);*/
  }
}

